clc;clear;close all;
field_limits = [0 100 0 100 0 20];
r_num = 3; d_num = 4; 
datafile = 'Results/MultiDensityCase/robot_his.mat';
load(datafile);
stablePos = cell(d_num,1); duration = 700;
for i = 1:d_num
    stablePos{i} = robot_his{duration*(i)+1};
end
stablePos = cell2mat(stablePos);
robot_his = cell2mat(robot_his);
robot_int = robot_his(1:r_num,:);
robot_end = robot_his(end-r_num+1:end,:);

time = length(robot_his)/r_num; scattersize1=30;
c = ['r','g','m'];

centers = [20 20; 80 20; 80 80; 20 80; ]; c_index = 1;
d_center = centers(c_index,:);
field_limits1 = field_limits;
discretization_num = 500; mesh_size = field_limits(2)/discretization_num;
x_space = linspace(field_limits1(1),field_limits1(2),discretization_num);
y_space = linspace(field_limits1(3),field_limits1(4),discretization_num);
[X_points,Y_points] = meshgrid(x_space,y_space);
points_xy = [reshape(X_points,discretization_num^2,1) reshape(Y_points,discretization_num^2,1)];
R = densityFunc(d_center,points_xy); j = 1;
f = figure('Name','VideoMaker'); hold on; grid on; view(-40,20); axis(field_limits); f.Visible = 'off';
set(f,'position',[1 1 860 460]); hold on;
for t = 1:1:time  
    t/time
    if mod(t-1,duration) == 0
        d_center = centers(c_index,:);
        R = densityFunc(d_center,points_xy);
        c_index = c_index + 1;
        if c_index > 4 
            c_index = 4;
        end
    end   
    %f = figure('Name','VideoMaker'); hold on; grid on; view(-40,20); axis(field_limits); f.Visible = 'off';
    pos = robot_his(r_num*(t-1)+1:r_num*(t),:);
    [X,Y,Z] = computeMeshforPlot(pos,field_limits,150);
    pcolor(X,Y,Z); shading interp; colorbar;caxis manual; caxis([0 1]);
    for i = 1:r_num
        scatter3(pos(i,1),pos(i,2),abs(pos(i,3)),scattersize1,'o',c(i),'filled');
    end
    scatter3(d_center(1),d_center(2),0,4*scattersize1,'^','m','filled');
    F(j) = getframe(f);
    clf(f); hold on; grid on; view(-40,20); axis(field_limits);
    j = j+1;   
end

save F_raw_dynamiccase.mat 'F';
%%
% load('F_raw_dynamiccase.mat');
writerObj = VideoWriter('DynamicDensityCase.avi');
writerObj.FrameRate = 60;
open(writerObj);

writeVideo(writerObj, F)
close(writerObj);
clear F;

%%
clc;
f4 = figure('Name','Trajectories3D'); view(161,51); axis(field_limits); hold on; grid on;f4.Visible = 'off';
robot = robot_his;
c = ['r','g','m']; scattersize1= 30;scattersize2 = 60;
rang{1} = 1:r_num:length(robot);
rang{2} = 2:r_num:length(robot);
rang{3} = 3:r_num:length(robot);
for i = 1:r_num
    scatter3(robot_int(i,1),robot_int(i,2),abs(robot_int(i,3)),scattersize1,'o',c(i),'filled');
    scatter3(stablePos(i:r_num:length(stablePos),1),stablePos(i:r_num:length(stablePos),2),abs(stablePos(i:r_num:length(stablePos),3)),scattersize2,'v',c(i),'filled');
    scatter3(robot(rang{i},1),robot(rang{i},2),abs(robot(rang{i},3)),1,'.',c(i));
end
j = 0;
for i = 1:360
    j = j+1;
    j/360
    view(161+i,51);
    T(j) = getframe(f4);
end

save T_diffviews_Dynamiccase.mat 'T';
writerObj1 = VideoWriter('TrajecotriesFromDiffViewDynamicCase.avi');
writerObj1.FrameRate = 10;
open(writerObj1);

writeVideo(writerObj1, T)
close(writerObj1);

clear T;




